Example: Path planning problem solved by Uniform grid abstraction.

This example was borrowed from (Reissig et al., 2016; IX. Examples, A) whose dynamics comes from the model given in (Åström and Murray, 2007; Ch. 2.4). This is a reachability problem for a continuous system.

Let us consider the 3-dimensional state space control system of the form

\[\dot{x} = f(x, u)\]

with $f: \mathbb{R}^3 × U ↦ \mathbb{R}^3$ given by

\[f(x,(u_1,u_2)) = \begin{bmatrix} u_1 \cos(α+x_3)\cos(α)^{-1} \\ u_1 \sin(α+x_3)\cos(α)^{-1} \\ u_1 \tan(u_2) \end{bmatrix}\]

and with $U = [−1, 1] \times [−1, 1]$ and $α = \arctan(\tan(u_2)/2)$. Here, $(x_1, x_2)$ is the position and $x_3$ is the orientation of the vehicle in the 2-dimensional plane. The control inputs $u_1$ and $u_2$ are the rear wheel velocity and the steering angle. The control objective is to drive the vehicle which is situated in a maze made of obstacles from an initial position to a target position.

In order to study the concrete system and its symbolic abstraction in a unified framework, we will solve the problem for the sampled system with a sampling time $\tau$. For the construction of the relations in the abstraction, it is necessary to over-approximate attainable sets of a particular cell. In this example, we consider the used of a growth bound function (Reissig et al., 2016; VIII.2, VIII.5) which is one of the possible methods to over-approximate attainable sets of a particular cell based on the state reach by its center.

For this reachability problem, the abstraction controller is built by solving a fixed-point equation which consists in computing the pre-image of the target set.

First, let us import StaticArrays and Plots.

using StaticArrays, Plots

At this point, we import Dionysos and JuMP.

using Dionysos, JuMP

Definition of the problem

We define the problem using JuMP as follows. We first create a JuMP model:

model = Model(Dionysos.Optimizer)

A JuMP Model
├ solver: unknown
├ objective_sense: FEASIBILITY_SENSE
├ num_variables: 0
├ num_constraints: 0
└ Names registered in the model: none

Define the state variables: x1(t), x2(t), x3(t)

x_low, x_upp = [0.0, 0.0, -pi - 0.4], [4.0, 10.0, pi + 0.4]
x_start = [0.4, 0.4, 0.0]
@variable(model, x_low[i] <= x[i = 1:3] <= x_upp[i], start = x_start[i])

3-element Vector{JuMP.VariableRef}:
 x[1]
 x[2]
 x[3]

Define the control variables: u1(t), u2(t)

@variable(model, -1 <= u[1:2] <= 1)

2-element Vector{JuMP.VariableRef}:
 u[1]
 u[2]

Set α(t) = arctan(tan(u2(t)) / 2)

@expression(model, α, atan(tan(u[2]) / 2))

@constraint(model, ∂(x[1]) == u[1] * cos(α + x[3]) * sec(α))
@constraint(model, ∂(x[2]) == u[1] * sin(α + x[3]) * sec(α))
@constraint(model, ∂(x[3]) == u[1] * tan(u[2]))

x_target = [3.3, 0.5, 0]

@constraint(model, final(x[1]) in MOI.Interval(3.0, 3.6))
@constraint(model, final(x[2]) in MOI.Interval(0.3, 0.8))
@constraint(model, final(x[3]) in MOI.Interval(-100.0, 100.0))

\[ \textsf{final}\left({x_{3}}\right) \in [-100, 100] \]

Obstacle boundaries (provided)

x1_lb = [1.0, 2.2, 2.2]
x1_ub = [1.2, 2.4, 2.4]
x2_lb = [0.0, 0.0, 6.0]
x2_ub = [9.0, 5.0, 10.0]

3-element Vector{Float64}:
  9.0
  5.0
 10.0

Function to add rectangular obstacle avoidance constraints

for i in eachindex(x1_ub)
    @constraint(
        model,
        x[1:2] ∉ MOI.HyperRectangle([x1_lb[i], x2_lb[i]], [x1_ub[i], x2_ub[i]])
    )
end

Definition of the abstraction

Definition of the grid of the state-space on which the abstraction is based (origin x0 and state-space discretization h):

We define the growth bound function of $f$:

function jacobian_bound(u)
    β = abs(u[1] / cos(atan(tan(u[2]) / 2)))
    return StaticArrays.SMatrix{3, 3}(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, β, β, 0.0)
end
set_attribute(model, "jacobian_bound", jacobian_bound)

set_attribute(model, "time_step", 0.3)

x0 = SVector(0.0, 0.0, 0.0);
h = SVector(0.2, 0.2, 0.2);
set_attribute(model, "state_grid", Dionysos.Domain.GridFree(x0, h))

Definition of the grid of the input-space on which the abstraction is based (origin u0 and input-space discretization h):

u0 = SVector(0.0, 0.0);
h = SVector(0.3, 0.3);
set_attribute(model, "input_grid", Dionysos.Domain.GridFree(u0, h))

optimize!(model)

>>Setting up the model
>>Model setup complete
┌ Warning: Noise is not now taken into account!
└ @ Dionysos.Optim.Abstraction.UniformGridAbstraction ~/.julia/packages/Dionysos/r5OrT/src/optim/abstraction/uniform_grid_abstraction.jl:229
compute_symmodel_from_controlsystem! started
compute_symmodel_from_controlsystem! terminated with success: 9705631 transitions created
  2.814228 seconds (90.67 k allocations: 877.409 MiB, 2.01% gc time, 3.63% compilation time)
┌ Warning: The `state_cost` and `transition_cost` is not yet fully implemented
└ @ Dionysos.Optim.Abstraction.UniformGridAbstraction ~/.julia/packages/Dionysos/r5OrT/src/optim/abstraction/uniform_grid_abstraction.jl:288
compute_controller_reach! started

compute_controller_reach! terminated with success

Get the results

abstract_system = get_attribute(model, "abstract_system");
abstract_problem = get_attribute(model, "abstract_problem");
abstract_controller = get_attribute(model, "abstract_controller");
concrete_controller = get_attribute(model, "concrete_controller")
concrete_problem = get_attribute(model, "concrete_problem");
concrete_system = concrete_problem.system

MathematicalSystems.ConstrainedBlackBoxControlContinuousSystem{RuntimeGeneratedFunctions.RuntimeGeneratedFunction{(:ˍ₋arg1, :ˍ₋arg2), Symbolics.var"#_RGF_ModTag", Symbolics.var"#_RGF_ModTag", (0x654fcf54, 0x3c39a3bf, 0x6382cc45, 0x73ff0455, 0x1e9d70c7), Expr}, Dionysos.Utils.LazySetMinus{Dionysos.Utils.HyperRectangle{StaticArraysCore.SVector{3, Float64}}, Dionysos.Utils.LazyUnionSetArray{Dionysos.Utils.HyperRectangle{StaticArraysCore.SVector{3, Float64}}}}, Dionysos.Utils.HyperRectangle{StaticArraysCore.SVector{2, Float64}}}(RuntimeGeneratedFunctions.RuntimeGeneratedFunction{(:ˍ₋arg1, :ˍ₋arg2), Symbolics.var"#_RGF_ModTag", Symbolics.var"#_RGF_ModTag", (0x654fcf54, 0x3c39a3bf, 0x6382cc45, 0x73ff0455, 0x1e9d70c7), Expr}(:(#= /home/runner/.julia/packages/Symbolics/PxO3a/src/build_function.jl:340 =# @inbounds begin
          #= /home/runner/.julia/packages/Symbolics/PxO3a/src/build_function.jl:340 =#
          begin
              #= /home/runner/.julia/packages/SymbolicUtils/jf8aQ/src/code.jl:385 =#
              #= /home/runner/.julia/packages/SymbolicUtils/jf8aQ/src/code.jl:386 =#
              #= /home/runner/.julia/packages/SymbolicUtils/jf8aQ/src/code.jl:387 =#
              begin
                  let var"var-176030304511817610131" = (+)(ˍ₋arg1[3], (atan)((*)(0.5, NaNMath.tan(ˍ₋arg2[2])))), var"var-176030304511817610132" = ˍ₋arg2[1], var"var-176030304511817610133" = (sec)((atan)((*)(0.5, NaNMath.tan(ˍ₋arg2[2])))), var"var-176030304511817610134" = NaNMath.tan(ˍ₋arg2[2])
                      #= /home/runner/.julia/packages/SymbolicUtils/jf8aQ/src/code.jl:480 =#
                      (SymbolicUtils.Code.create_array)(typeof(ˍ₋arg1), nothing, Val{1}(), Val{(3,)}(), (*)((*)(var"var-176030304511817610132", var"var-176030304511817610133"), NaNMath.cos(var"var-176030304511817610131")), (*)((*)(var"var-176030304511817610132", var"var-176030304511817610133"), NaNMath.sin(var"var-176030304511817610131")), (*)(var"var-176030304511817610132", var"var-176030304511817610134"))
                  end
              end
          end
      end)), 3, 2, Dionysos.Utils.LazySetMinus{Dionysos.Utils.HyperRectangle{StaticArraysCore.SVector{3, Float64}}, Dionysos.Utils.LazyUnionSetArray{Dionysos.Utils.HyperRectangle{StaticArraysCore.SVector{3, Float64}}}}(Dionysos.Utils.HyperRectangle{StaticArraysCore.SVector{3, Float64}}([0.0, 0.0, -3.541592653589793], [4.0, 10.0, 3.541592653589793]), Dionysos.Utils.LazyUnionSetArray{Dionysos.Utils.HyperRectangle{StaticArraysCore.SVector{3, Float64}}}(Dionysos.Utils.HyperRectangle{StaticArraysCore.SVector{3, Float64}}[Dionysos.Utils.HyperRectangle{StaticArraysCore.SVector{3, Float64}}([1.0, 0.0, -Inf], [1.2, 9.0, Inf]), Dionysos.Utils.HyperRectangle{StaticArraysCore.SVector{3, Float64}}([2.2, 0.0, -Inf], [2.4, 5.0, Inf]), Dionysos.Utils.HyperRectangle{StaticArraysCore.SVector{3, Float64}}([2.2, 6.0, -Inf], [2.4, 10.0, Inf])])), Dionysos.Utils.HyperRectangle{StaticArraysCore.SVector{2, Float64}}([-1.0, -1.0], [1.0, 1.0]))

Trajectory display

We choose a stopping criterion reached and the maximal number of steps nsteps for the sampled system, i.e. the total elapsed time: nstep*tstep as well as the true initial state x0 which is contained in the initial state-space _I_ defined previously.

nstep = 100
function reached(x)
    if x ∈ concrete_problem.target_set
        return true
    else
        return false
    end
end

x0 = SVector(0.4, 0.4, 0.0)
control_trajectory = Dionysos.System.get_closed_loop_trajectory(
    get_attribute(model, "discretized_system"),
    concrete_controller,
    x0,
    nstep;
    stopping = reached,
)

using Plots

Here we display the coordinate projection on the two first components of the state space along the trajectory.

fig = plot(; aspect_ratio = :equal);

We display the concrete domain

plot!(concrete_system.X; color = :yellow, opacity = 0.5);

We display the abstract domain

plot!(abstract_system.Xdom; color = :blue, opacity = 0.5);

We display the concrete specifications

plot!(concrete_problem.initial_set; color = :green, opacity = 0.2);
plot!(concrete_problem.target_set; dims = [1, 2], color = :red, opacity = 0.2);

We display the abstract specifications

plot!(
    Dionysos.Symbolic.get_domain_from_symbols(
        abstract_system,
        abstract_problem.initial_set,
    );
    color = :green,
);
plot!(
    Dionysos.Symbolic.get_domain_from_symbols(abstract_system, abstract_problem.target_set);
    color = :red,
);

We display the concrete trajectory

plot!(control_trajectory; ms = 0.5)

This page was generated using Literate.jl.